#!/usr/bin/env python3
# -*-coding:utf8-*-
# 注意demo无法直接运行，需要pip安装sdk后才能运行
from typing import (
    Optional,
)
import time
from piper_sdk import *
import numpy as np

def enable_fun(piper:C_PiperInterface_V2):
    '''
    使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
    '''
    enable_flag = False
    # 设置超时时间（秒）
    timeout = 5
    # 记录进入循环前的时间
    start_time = time.time()
    elapsed_time_flag = False
    while not (enable_flag):
        elapsed_time = time.time() - start_time
        print("--------------------")
        enable_flag = piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status and \
            piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status and \
            piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status and \
            piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status and \
            piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status and \
            piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status
        print("使能状态:",enable_flag)
        piper.EnableArm(7)
        piper.GripperCtrl(0,1000,0x01, 0)
        print("--------------------")
        # 检查是否超过超时时间
        if elapsed_time > timeout:
            print("超时....")
            elapsed_time_flag = True
            enable_flag = True
            break
        time.sleep(1)
        pass
    if(elapsed_time_flag):
        print("程序自动使能超时,退出程序")
        exit(0)

if __name__ == "__main__":
    piper = C_PiperInterface_V2(can_name="right")
    piper.ConnectPort()
    piper.EnableArm(7)
    enable_fun(piper=piper)
    # piper.DisableArm(7)
    piper.GripperCtrl(0,1000,0x01, 0)
    factor = 57295.7795 #1000*180/3.1415926
    # factor =1
    # position = [0.0758,0.5812,-0.0489,-0.0034,-0.8671,-0.0047,0] # 弧度
    position = np.array([ 0.,0,0,0,0,0,0])
    # print("degree:",np.array(position)*180/3.1415926)

        
    joint_0 = round(position[0]*factor)
    joint_1 = round(position[1]*factor)
    joint_2 = round(position[2]*factor)
    joint_3 = round(position[3]*factor)
    joint_4 = round(position[4]*factor)
    joint_5 = round(position[5]*factor)
    joint_6 = round(position[6]*1000*1000)
    # piper.MotionCtrl_1()
    piper.MotionCtrl_2(0x01, 0x01, 100, 0x00)
    # 单位 微度
    piper.JointCtrl(joint_0, joint_1, joint_2, joint_3, joint_4, joint_5)
    piper.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
    print(piper.GetArmStatus())
    # 获取关节位置
    joint_msgs = piper.GetArmJointMsgs()
    
    # 提取位置信息（单位：度）并转换为弧度
    positions = np.zeros(6)
    positions[0] = joint_msgs.joint_state.joint_1 * 0.001 * np.pi / 180.0
    positions[1] = joint_msgs.joint_state.joint_2 * 0.001 * np.pi / 180.0
    positions[2] = joint_msgs.joint_state.joint_3 * 0.001 * np.pi / 180.0
    positions[3] = joint_msgs.joint_state.joint_4 * 0.001 * np.pi / 180.0
    positions[4] = joint_msgs.joint_state.joint_5 * 0.001 * np.pi / 180.0
    positions[5] = joint_msgs.joint_state.joint_6 * 0.001 * np.pi / 180.0
    
    print("positions:",positions) # 弧度
    time.sleep(0.005)
    pass